'''
本程序实现串口实时接收数据的功能，并且接收数据成功后回复OK
如果提示缺少串口模块，需要用以下命令安装pyserial
$ pip install pyserial
传输指令
scp -r uart user192.168.137.136:/home/user/wk
'''

import serial
import math
from time import sleep

def recv(S, fps=20):
    assert isinstance(S, serial.Serial), "serial变量必须是serial.Serial(串口)数据类型"
    buffer = bytearray()
    start = 0

    while True:
        buffer.extend(S.read_all())

        # 保证缓冲区不会无限增长
        if len(buffer) > 48:
            buffer = buffer[-48:]

        # 查找所有可能的帧头
        idx_head = buffer.find(0x7B, start)
        if idx_head == -1 or idx_head + 24 > len(buffer):
            break
        if buffer[idx_head + 23] == 0x7D:
            frame = buffer[idx_head:idx_head + 24]
            # 异或校验
            check = 0
            for i in range(23):
                check ^= frame[i]
            if check == 0:
                # 校验通过，移除已处理数据并返回
                del buffer[:idx_head + 24]
                return recv_data_process(frame)
            else:
                print(f"check:{check}, 22:{frame[22]}")

        if idx_head == -1:
            start = 24
        elif idx_head >= 24:
            start = idx_head - 24
        else:
            start = idx_head

def send(S, data):
  assert len(data) == 11, f"数据长度不正确:{len(data)}"
  try:
    # 发送字节数据
    S.write(data)  # 发送数据
  except Exception as e:
    print(f"串口发送失败:{e}")

# 将两个二进制字节合并并转化为int类型,第一个为高八位,第二个为低八位
def comb(high_byte, low_byte):
  raw = (high_byte << 8) | low_byte
  
  # 高八位最高位为1表示为负,前面所有位都应当为1
  if high_byte & 0x80:
     raw = -((~raw + 1) & 0xffff)
  return raw

def recv_data_process(frame):
  assert len(frame) == 24, f"数据长度不正确{len(frame)}"

  # 处理数据
  C = 0.001
  v_x = C * comb(frame[2], frame[3])  # 纵向速度m/s
  v_y = C * comb(frame[4], frame[5])  # 横向速度吗m/s
  v_z = C * comb(frame[6], frame[7])  # 垂直速度m/s

  g = 9.8
  C = g / 2 ** 14
  a_x = C * comb(frame[8], frame[9])  # 纵向加速度m/s^2
  a_y = C * comb(frame[10], frame[11])  # 横向加速度m/s^2
  a_z = C * comb(frame[12], frame[13])  # 角加速度rad/s^2

  C = (500 / 2 ** 15) / (180 / math.pi)
  r_x = C * comb(frame[14], frame[15])  # x轴角速度rad/s
  r_y = C * comb(frame[16], frame[17])  # y轴角速度rad/s
  r_z = C * comb(frame[18], frame[19])  # z轴角速度rad/s

  vol = 0.001 * comb(frame[20], frame[21]) / 2  # 电池电压V

  data = {
    'v_x': v_x,
    'v_y': v_y,
    'v_z': v_z,
    'r_x': r_x,
    'r_y': r_y,
    'r_z': r_z,
    'a_x': a_x,
    'a_y': a_y,
    'a_z': a_z,
    'g': g,
    'vol': vol
  }
  return data

# 处理数据,data有数据v_x,v_y,r_z
# 帧头为0x7B,帧尾为0x7D,加上异或位校验,返回而二进制字节序列
def send_data_process(data):
  assert isinstance(data, dict), "数据类型错误"
  assert 'v_x' in data and 'v_y' in data and 'r_z' in data, "数据格式错误"

  frame = bytearray()
  frame.append(0x7B)  # 帧头
  frame.append(0x00)  # 预留位
  frame.append(0x00)  # 预留位

  # 加高八位
  frame.append(data['v_x'] >> 8 & 0xFF)
  # 加低八位
  frame.append(data['v_x'] & 0xFF)

  frame.append(data['v_y'] >> 8 & 0xFF)
  frame.append(data['v_y'] & 0xFF)
  frame.append(data['r_z'] >> 8 & 0xFF)
  frame.append(data['r_z'] & 0xFF)

  # 加异或校验
  check = 0
  for i in range(len(frame)):
    check ^= frame[i]
  frame.append(check & 0xFF)  # 异或校验位

  frame.append(0x7D) # 帧尾

  return frame

def output_data(data):
    i = 0
    for name, value in data.items():
      if i % 3 == 2:
        print(f"{name}: {value}")
      else:
        print(f", {name}: {value}", end = " | ")
      i += 1
    if i % 3 != 0:
       print(" ")
    print(" ")

def read(S, fps = 20, timeout = 0):
  try:
    if timeout == 0:
      while True:
        data = recv(S)
        sleep(1/fps)
        output_data(data) # 接收成功
    else:
      i = 0
      while i < timeout * fps:
        data = recv(S)
        sleep(1/fps)
        output_data(data) # 接收成功
        i += 1
  except Exception as e:
    print(f"串口异常:{e}")

# 设置小车目标速率
def write(S, v_x = 0, v_y = 0 ,r_z = 0):
   try:
      data = {
        'v_x': v_x,
        'v_y': v_y,
        'r_z': r_z
      }
      # 单位为mm/s，rad/s
      send(S, send_data_process(data))
   except Exception as e:
     print(f"串口异常:{e}")


if __name__ == '__main__':
    try:
        S = serial.Serial('/dev/ttyACM0', 115200, timeout=5)
        if S.isOpen():
            print("open success")
        else:
            print("open failed")

        write(S)
        read(S, timeout=0.3)
        write(S, v_x=100, r_z=100)
        read(S, timeout=0.3)
        write(S)
    except Exception as e:
        print(f"串口异常:{e}")
    finally:
        S.close()
